package com.jmex.physics.impl.bullet;

import java.util.ArrayList;
import java.util.List;

import com.jme.math.Quaternion;
import com.jme.math.Vector3f;
import com.jme.scene.Spatial;
import com.jmex.physics.DynamicPhysicsNode;
import com.jmex.physics.impl.bullet.geometry.BulletGeometry;
import com.jmex.physics.material.Material;

public class BulletDynamicPhysicsNode extends DynamicPhysicsNode {

	private BulletPhysicsSpace space;
	private BulletRigidBody body;
	
	private float mass = 1f;
	private Vector3f centerOfMass = new Vector3f();
	
    private List<BulletGeometry> collisionShapes = new ArrayList<BulletGeometry>();
    private boolean dirty;

    private Vector3f tempScale = new Vector3f();
    private Vector3f tempPos = new Vector3f();
    
    private Vector3f lastTranslation = new Vector3f();
    private Quaternion lastRotation = new Quaternion();
    private Vector3f lastLinearVelocity = new Vector3f();
    private Vector3f lastAngularVelocity = new Vector3f();
    
    private Vector3f linearVelocity = new Vector3f();
    private Vector3f angularVelocity = new Vector3f();
    
    private List<Vector3f[]> appliedForces = new ArrayList<Vector3f[]>();
    
    public BulletDynamicPhysicsNode( BulletPhysicsSpace space ) {
        this.space = space;
    }

    @Override
    public int attachChild( Spatial child ) {
        int index = super.attachChild( child );
        if ( child instanceof BulletGeometry ) {
            dirty = true;
            collisionShapes.add( (BulletGeometry) child );
        }
        return index;
    }

    @Override
    public int detachChild( Spatial child ) {
        int index = super.detachChild( child );
        if ( child instanceof BulletGeometry ) {
            dirty = true;
            collisionShapes.remove( child );
        }
        return index;
    }

    public boolean isTouched()
    {
    	if(body==null)
    		return false;
    	return !(getWorldTranslation().equals(lastTranslation) &&
    			getWorldRotation().equals(lastRotation) &&
    			getLinearVelocity(tempPos).equals(lastLinearVelocity) &&
    			getAngularVelocity(tempPos).equals(lastAngularVelocity));
    }

    void sync()
    {
    	lastTranslation.set(getWorldTranslation());
    	lastRotation.set(getWorldRotation());
    	getLinearVelocity(lastLinearVelocity);
    	getAngularVelocity(lastAngularVelocity);
    }

    List<Vector3f[]> getAppliedForces()
    {
    	return appliedForces;
    }
    
    public void rebuildRigidBody() {

        updateWorldVectors();

        if(body==null)
        	body = new BulletRigidBody(this,collisionShapes);
        else
        	body.setCollisionShapes(collisionShapes);
        
        body.updateRigidBody();
        
        sync();
        dirty=false;
        for(BulletGeometry geom : collisionShapes)
        	geom.setDirty(false);
    }

	public BulletRigidBody getBody() {
		return body;
	}

    @Override
	public void addForce(Vector3f force) {
    	appliedForces.add(new Vector3f[]{force,Vector3f.ZERO,Vector3f.ZERO});
	}

	@Override
	public void addForce(Vector3f force, Vector3f at) {
    	appliedForces.add(new Vector3f[]{force,at,Vector3f.ZERO});
	}

	@Override
	public void addTorque(Vector3f torque) {
		appliedForces.add(new Vector3f[]{Vector3f.ZERO,Vector3f.ZERO,torque});
	}

	@Override
	public void clearForce() {
		appliedForces.add(new Vector3f[]{null,null,Vector3f.ZERO});
	}

	@Override
	public void clearTorque() {
		appliedForces.add(new Vector3f[]{Vector3f.ZERO,Vector3f.ZERO,null});
	}

	@Override
	public void computeMass() {
        mass = 0f;
        float tempMass;
        centerOfMass=new Vector3f();
        Material mat;
        for ( BulletGeometry geom : collisionShapes ) {
            getWorldScale().mult( geom.getLocalScale(), tempScale );
            tempPos.set(geom.getLocalTranslation().divide( tempScale ));
        	mat = geom.getMaterial();
        	if(mat==null) mat=getMaterial();
            tempMass = geom.getVolume() * mat.getDensity();
            mass += tempMass;
            if(mass>0)
            	centerOfMass.interpolate(tempPos, tempMass/mass);
        }
        if(mass==0)
        	mass=1f;
        dirty = true;
	}

	@Override
	public Vector3f getAngularVelocity(Vector3f store) {
		if(store==null)
			store=new Vector3f();
		store.set(angularVelocity);
		return store;
	}

	@Override
	public Vector3f getCenterOfMass(Vector3f store) {
		if(store==null)
			store=new Vector3f();
		store.set(centerOfMass);
		return store;
	}

	@Override
	public Vector3f getForce(Vector3f store) {
		if(store==null)
			store=new Vector3f();
		store.set(Vector3f.ZERO);
		return store;
	}

	@Override
	public Vector3f getLinearVelocity(Vector3f store) {
		if(store==null)
			store=new Vector3f();
		store.set(linearVelocity);
		return store;
	}

	@Override
	public float getMass() {
		return mass;
	}

	@Override
	public Vector3f getTorque(Vector3f store) {
		if(store==null)
			store=new Vector3f();
		store.set(Vector3f.ZERO);
		return store;
	}

	@Override
	public boolean isAffectedByGravity() {
		// TODO Auto-generated method stub
		return false;
	}

	@Override
	public void rest() {
		// TODO Auto-generated method stub

	}

	@Override
	public void setAffectedByGravity(boolean value) {
		// TODO Auto-generated method stub

	}

	@Override
	public void setCenterOfMass(Vector3f value) {
		if(value==null)
			return;
		centerOfMass.set(value);
	}

	@Override
	public void setMass(float value) {
		mass=value;
	}

	@Override
	public void unrest() {
		// TODO Auto-generated method stub

	}

	@Override
	public BulletPhysicsSpace getSpace() {
		return space;
	}

	@Override
	public boolean isResting() {
		// TODO Auto-generated method stub
		return false;
	}

    public boolean isDirty() {
        if ( dirty ) {
            return true;
        }
        for ( BulletGeometry geom : this.collisionShapes ) {
            if ( geom.isDirty() ) {
                return true;
            }
        }
        return false;
    }

	@Override
	public void setAngularVelocity(Vector3f velocity) {
		if(velocity==null)
			return;
		angularVelocity.set(velocity);
	}

	@Override
	public void setLinearVelocity(Vector3f velocity) {
		if(velocity==null)
			return;
		linearVelocity.set(velocity);
	}

}
